#include "steadywin.h"
#include "can_common.h"
#include "motor_def.h"
#include "offline.h"
#include "rtthread.h"
#include "rttypes.h"


static uint8_t idx;
static rt_slist_t Steadywin_motor_list= {RT_NULL}; // 会在motor_task任务中遍历该指针数组进行pid计算

#define LOG_TAG              "steadywin"
#define LOG_LVL              LOG_LVL_DBG
#include <ulog.h>
rt_inline void steadywin_motor_list_init(rt_slist_t *l)
{
    l->next = RT_NULL;//0
}

rt_inline void steadywin_motor_list_append(rt_slist_t *l, rt_slist_t *n)
{
    struct rt_slist_node *node;

    node = l;
    while (node->next) node = node->next;

    /* append the node to the tail */
    node->next = n;
    n->next = RT_NULL;
}
rt_inline void list_append(SteadyWinMotor_t *device)
{
    steadywin_motor_list_init(&(device->list));
    steadywin_motor_list_append(&Steadywin_motor_list, &(device->list));
}


void SteadyWinMotorSetCmd(SteadyWinMotor_Mode_e cmd, SteadyWinMotor_t *motor)
{
    
    // rt_memset(can_msg.data,0xff,7); // 发送电机指令的时候前面7bytes都是0xff
    motor->CanMessage.txmsg.data[0]=0xff;
    motor->CanMessage.txmsg.data[1]=0xff;
    motor->CanMessage.txmsg.data[2]=0xff;
    motor->CanMessage.txmsg.data[3]=0xff;
    motor->CanMessage.txmsg.data[4]=0xff;
    motor->CanMessage.txmsg.data[5]=0xff;
    motor->CanMessage.txmsg.data[6]=0xff;
    motor->CanMessage.txmsg.data[7] = (uint8_t)cmd; // 最后一位是命令id
    motor->CanMessage.txmsg.id = motor->CanMessage.tx_id + 0x000;
    motor->CanMessage.txmsg.ide = RT_CAN_STDID;
    motor->CanMessage.txmsg.rtr =RT_CAN_DTR;
    motor->CanMessage.txmsg.len = 8;
    can_send(*motor->CanMessage.can_handle,&motor->CanMessage.txmsg);
    //rt_device_write(*motor->CanMessage.can_handle,0,&motor->CanMessage.txmsg,sizeof(motor->CanMessage.txmsg));
}

void SteadyWin_mit_ctrl(SteadyWinMotor_t *motor,float pos, float vel, float kp, float kd, float torq, float torque_constant, float gear_ratio)
{
    uint16_t pos_tmp, vel_tmp, kp_tmp, kd_tmp, tor_tmp;

    // Convert float values to uint16_t using the provided formulas
    pos_tmp = (rt_uint16_t)((pos + 12.5) * 65535 / 25);
    vel_tmp = (rt_uint16_t)((vel + 65) * 4095 / 130);
    kp_tmp =  (rt_uint16_t)(kp * 4095 / 500);
    kd_tmp =  (rt_uint16_t)(kd * 4095 / 5);
    tor_tmp = (rt_uint16_t)((torq + 225 * torque_constant * gear_ratio) * 4095 / (450 * torque_constant * gear_ratio));

    // Pack the data into the CAN message
    motor->CanMessage.txmsg.id  = motor->CanMessage.tx_id +0x000;
    motor->CanMessage.txmsg.ide = RT_CAN_STDID;
    motor->CanMessage.txmsg.rtr = RT_CAN_DTR;
    motor->CanMessage.txmsg.len = 8;

    motor->CanMessage.txmsg.data[0] = (pos_tmp >> 8) & 0xFF; // BYTE0: 位置高8位
    motor->CanMessage.txmsg.data[1] = pos_tmp & 0xFF;       // BYTE1: 位置低8位
    motor->CanMessage.txmsg.data[2] = (vel_tmp >> 4) & 0xFF; // BYTE2: 速度高8位
    motor->CanMessage.txmsg.data[3] = ((vel_tmp & 0xF) << 4) | (kp_tmp >> 8); // BYTE3: 速度低4位 + KP高4位
    motor->CanMessage.txmsg.data[4] = kp_tmp & 0xFF;        // BYTE4: KP低8位
    motor->CanMessage.txmsg.data[5] = (kd_tmp >> 4) & 0xFF;   // BYTE5: KD高8位
    motor->CanMessage.txmsg.data[6] = ((kd_tmp & 0xF) << 4) | (tor_tmp >> 8); // BYTE6: KD低4位 + 力矩高4位
    motor->CanMessage.txmsg.data[7] = tor_tmp & 0xFF;       // BYTE7: 力矩低8位

    can_send(*motor->CanMessage.can_handle,&motor->CanMessage.txmsg);
    //rt_device_write(*motor->CanMessage.can_handle,0,&motor->CanMessage.txmsg,sizeof(motor->CanMessage.txmsg));
}


void SteadyWinMotorDecode(struct rt_can_msg can_msg)
{
    rt_slist_t *node = RT_NULL;
    SteadyWinMotor_t *swdevice = RT_NULL;

    rt_slist_for_each(node, &Steadywin_motor_list)
    {
        swdevice = rt_slist_entry(node,SteadyWinMotor_t,list);
        if (swdevice == RT_NULL)
        {
            LOG_E("Invalid dmdevice pointer\n");
            continue;
        }
        if (can_msg.id == swdevice->CanMessage.rx_id)
        {
            rt_int16_t tmp;

            // 解析异常码
            uint8_t exception_code = can_msg.data[1];

            if (exception_code == 0x00)
            {
                // 无异常，重置错误计数器
                swdevice->measure.last_error_code = 0x00;
                swdevice->measure.error_count = 0;
                swdevice->measure.Error_Code = SteadyWin_NO_ERROR;
            }
            else
            {
                if (exception_code == swdevice->measure.last_error_code)
                {
                    // 相同的错误码，增加计数器
                    swdevice->measure.error_count++;
                    if (swdevice->measure.error_count >= 200)
                    {
                        // 达到阈值，认为出现故障
                        switch (exception_code)
                        {
                            case 0x01:
                                swdevice->measure.Error_Code = FoC_frequency_too_high_ERROR;
                                break;
                            case 0x02:
                                swdevice->measure.Error_Code = Overvoltage_ERROR;
                                break;
                            case 0x04:
                                swdevice->measure.Error_Code = Undervoltage_ERROR;
                                break;
                            case 0x08:
                                swdevice->measure.Error_Code = Overtemperature_ERROR;
                                break;
                            case 0x10:
                                swdevice->measure.Error_Code = Startup_failed_ERROR;
                                break;
                            case 0x40:
                                swdevice->measure.Error_Code = Overcurrent_ERROR;
                                break;
                            case 0x80:
                                swdevice->measure.Error_Code = Software_fault_ERROR;
                                break;
                            default:
                                swdevice->measure.Error_Code = SteadyWin_NO_ERROR; // 未知错误码，清除错误码
                                break;
                        }
                        swdevice->measure.error_count = 0;
                    }
                }
                else
                {
                    // 不同的错误码，重置计数器
                    swdevice->measure.last_error_code = exception_code;
                    swdevice->measure.error_count = 1;
                    swdevice->measure.Error_Code = SteadyWin_NO_ERROR;
                }
            }

            // 解析位置
            tmp = (can_msg.data[1] << 8) | can_msg.data[2];
            swdevice->measure.position = tmp * (25.0f / 65535.0f) - 12.5f;

            // 解析速度
            tmp = (can_msg.data[3] << 4) | ((can_msg.data[4] >> 4) & 0x0F);
            swdevice->measure.velocity = tmp * (130.0f / 4095.0f) - 65.0f;

            // 解析力矩
            tmp = ((can_msg.data[4] & 0x0F) << 8) | can_msg.data[5];
            swdevice->measure.torque = tmp * (450.0f * swdevice->torque_constant * swdevice->gear_ratio) / 4095.0f - 225.0f * swdevice->torque_constant * swdevice->gear_ratio;
            
            offline_event_time_update((offline_event)swdevice->offline_manage_event);
        }
    }
}


SteadyWinMotor_t *SteadyWinMotorInit(Motor_Init_Config_s *config)
{
    SteadyWinMotor_t *motor = (SteadyWinMotor_t *)rt_malloc(sizeof(SteadyWinMotor_t));
    if (motor == RT_NULL) {
        LOG_E("Failed to allocate memory for DJIMotor\n");
        return RT_NULL;
    }
    rt_memset(motor, 0, sizeof(SteadyWinMotor_t));
    list_append(motor);

    motor->motor_type = config->motor_type;
    switch (motor->motor_type)
    {
        case SteadyWin_8115_9:
            motor->torque_constant = 0.11667f;
            motor->gear_ratio = 9;
            break;
        case SteadyWin_6110_36:
            motor->torque_constant = 0.08472f;
            motor->gear_ratio = 36;
            break;
        default:
            break;
    }
    motor->motor_settings = config->controller_setting_init_config;
    motor->CanMessage.can_handle = config->can_init_config.can_handle;
    motor->CanMessage.rx_id = config->can_init_config.rx_id;
    motor->CanMessage.tx_id = config->can_init_config.tx_id;
    motor->motor_settings.control_algorithm = config->controller_setting_init_config.control_algorithm;

    switch (config->controller_setting_init_config.control_algorithm) {
        case CONTROL_PID:
            // motor controller init 电机控制器初始化
            // motor controller init 电机控制器初始化
            PIDInit(&motor->motor_controller.current_PID, &config->controller_param_init_config.current_PID);
            PIDInit(&motor->motor_controller.speed_PID, &config->controller_param_init_config.speed_PID);
            PIDInit(&motor->motor_controller.angle_PID, &config->controller_param_init_config.angle_PID);
            motor->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
            motor->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
            motor->motor_controller.current_feedforward_ptr = config->controller_param_init_config.current_feedforward_ptr;
            motor->motor_controller.speed_feedforward_ptr = config->controller_param_init_config.speed_feedforward_ptr;
            break;
        case CONTROL_LQR:
            LQRInit(&motor->motor_controller.lqr, &config->controller_param_init_config.lqr_config);
            motor->motor_controller.other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr;
            motor->motor_controller.other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr;
            motor->motor_controller.current_feedforward_ptr = config->controller_param_init_config.current_feedforward_ptr;
            motor->motor_controller.speed_feedforward_ptr = config->controller_param_init_config.speed_feedforward_ptr;
            break;
        case CONTROL_OTHER:
            // 未来添加其他控制算法的初始化
            break;
    }

    //掉线检测
    motor->offline_manage_event = config->offline_manage_motor.event;
    offline_event_init(config->offline_manage_motor);

    SteadyWinMotorSetCmd(CMD_CLEAR_ERROR,motor);
    return motor;
}


void SteadyWinMotorSetRef(SteadyWinMotor_t *motor, float ref)
{
    switch (motor->motor_settings.control_algorithm) 
    {
        case CONTROL_PID:
            motor->motor_controller.pid_ref = ref;
            break;
        case CONTROL_LQR:
            motor->motor_controller.lqr_ref = ref;
            break;
        case CONTROL_OTHER:
            break;
    }
}

void SteadyWinMotorEnable(SteadyWinMotor_t *motor)
{
    motor->stop_flag = MOTOR_ENALBED;
    SteadyWinMotorSetCmd(CMD_MOTOR_MODE, motor);
}

void SteadyWinMotorStop(SteadyWinMotor_t *motor)
{
    motor->stop_flag = MOTOR_STOP;
    SteadyWinMotorSetCmd(CMD_RESET_MODE, motor); 
}

void SteadyWinMotorOuterLoop(SteadyWinMotor_t *motor, Closeloop_Type_e type)
{
    motor->motor_settings.outer_loop_type = type;
}


void SteadyWinMotorcontrol(void)
{
    SteadyWinMotor_t *motor =RT_NULL;
    float state0,state1;
    

    rt_slist_t *node = RT_NULL;
    rt_slist_for_each(node, &Steadywin_motor_list)
    {
        motor = rt_slist_entry(node,SteadyWinMotor_t,list);
        if (offline_manage[(offline_event)motor->offline_manage_event].online_state == STATE_ONLINE)
        {
            SteadyWin_mit_ctrl(motor,0,0,0,0,motor->motor_controller.lqr_ref,motor->torque_constant,motor->gear_ratio);
        }
        else
        {
            SteadyWinMotorSetCmd(CMD_RESET_MODE, motor);
        }

        
    }
}
